#include "jkcan.h"
#include <cstring>
#include "can/canitf.h"
#include "log/zlog.hh"
#include "log/datacollector.hh"

using namespace std;
using namespace rtbus;

#define jkcan_error(FMT, ...) zlog()->error("[jkcan] " FMT, ##__VA_ARGS__)
#define jkcan_info(FMT, ...) zlog()->info("[jkcan] " FMT, ##__VA_ARGS__)
#ifdef _DEBUG
#define jkcan_debug(FMT, ...)
#else
#define jkcan_debug(FMT, ...)
#endif

JkCan::JkCan(bool is_scan) : actived_flag_(false)
{
    drv_ = create_can(is_scan ? CANItf::CANType::SOCKETCAN : CANItf::CANType::CONTROLCAN);
    cyclic_time_us_ = 8000;  //FIXME: read from config file
}
JkCan::~JkCan() {}
int JkCan::open(BusCfg buscfg)
{
    CANItf::Config cfg;
    cfg.adapter_name = "can" + to_string(buscfg.channel_id);
    cfg.dev_id = buscfg.dev_id;
    cfg.can_id = buscfg.channel_id;
    int err = drv_->load_cfg(cfg);
    if (err)
    {
        return err;
    }
    err = drv_->active();
    if (err)
    {
        return err;
    }
    std::string name = drv_->type() == CANItf::CANType::SOCKETCAN ? "socketcan" : "controlcan";
    jkcan_info("Open bus:{} at {}-{}:{} success", name_, buscfg.channel_id, name, cfg.dev_id, cfg.can_id);
    return active();
}
int JkCan::active()
{
    lock_guard<mutex> l(sdo_mtx_);
    sdo_list_.clear();
    cur_sdo_.reset();
    actived_flag_ = true;
    return 0;
}
int JkCan::deactive()
{
    actived_flag_ = false;
    lock_guard<mutex> l(sdo_mtx_);
    sdo_list_.clear();
    cur_sdo_.reset();
    return 0;
}
void JkCan::close()
{
    deactive();
    drv_->deactive();
}

ssize_t JkCan::send_without_check(CANItf::FrameData* frame, size_t max_cnt)
{
    //FIXME(xxf): check frame size

    int tx_cnt = 0;
    //send txpdo or event
    for (auto& pdo : txpdo_)
    {
        frame[tx_cnt].id = pdo.first;
        frame[tx_cnt].dlc = pdo.second.size;
        memcpy(frame[tx_cnt].data, pdo.second.data, pdo.second.size);
        tx_cnt++;
    }

    //send sdo req
    auto sdo_req = get_sdo_req_data();
    if (sdo_req)
    {
        jkcan_debug("slave{} send frame: {} sdo{}", sdo_req->slave_id, sdo_req->is_write ? "write" : "read", sdo_req->obj_id);
        frame[tx_cnt].id = sdo_req->frame_req_id;
        frame[tx_cnt].dlc = sdo_req->frame_size;
        memcpy(frame[tx_cnt].data, sdo_req->frame_data, sdo_req->frame_size);
        tx_cnt++;
    }
    return tx_cnt;
}
void JkCan::send()
{
    if (!is_actived())
        return;
    const size_t SEND_FRAME_CNT = 20;
    CANItf::FrameData frame[SEND_FRAME_CNT];
    int tx_cnt = send_without_check(frame, SEND_FRAME_CNT);
    collect_can_data(true, frame, tx_cnt);
    drv_->send(frame, tx_cnt);
}
void JkCan::collect_can_data(bool is_tx, CANItf::FrameData* frame, size_t cnt)
{
    timespec cur_time;
    clock_gettime(CLOCK_MONOTONIC, &cur_time);
    uint64_t ct = (cur_time.tv_nsec + cur_time.tv_sec * 1000000000) / 100000;
    for (size_t i = 0; i < cnt; i++)
    {
        auto f = frame + i;
        // record frame
        zucDiagBusMsg msg;
        msg.is_ext = true;
        msg.is_tx = true;
        msg.size = f->dlc;
        msg.id = f->id;
        msg.mtnNo = 0;  //TODO: motion line
        msg.timestamp_ = ct;
        memcpy(msg.data, f->data, msg.size);
        zucDiagBus(msg);
    }
}
void JkCan::recv(int64_t* timeoff)
{
    if (!is_actived())
        return;
    CANItf::FrameData frame[200];
    int reclen = drv_->recv(frame, 200);
    if (reclen > 0)
    {
        collect_can_data(false, frame, reclen);
        for (int i = 0; i < reclen; i++)
        {
            auto f = &frame[i];
            auto pdo = rxpdo_.find(f->id);
            if (pdo != rxpdo_.end())
            {
                pdo->second.alive_cnt++;
                size_t minsize = f->dlc < pdo->second.size ? f->dlc : pdo->second.size;
                memcpy(pdo->second.data, f->data, minsize);
                continue;
            }
            // if (0 == rtbus::BusMgr::get()->try_rx_callback(bus_id_, f->id, f->data, f->dlc))  //pdo等处理
            // {
            //     continue;
            // }
            else if (cur_sdo_.get())  //sdo处理
            {
                //匹配frameid
                if (cur_sdo_->frame_resp_id != f->id)
                    continue;

                //匹配slaveid
                if (f->data[0] != cur_sdo_->slave_id)
                    continue;

                //匹配objid
                for (size_t i = 0; i < cur_sdo_->frame_obj_size; i++)
                {
                    if (f->data[i + 1] != cur_sdo_->frame_data[i + 1])
                        continue;
                }

                // 检查回调
                if (cur_sdo_->cb(f->data, f->dlc))
                {
                    continue;
                }
                // 复位当前sdo请求，cur_sdo_其会在get_sdo_req_data再次更新
                jkcan_debug("slave{} recv frame: sdo{} resp", cur_sdo_->slave_id, cur_sdo_->obj_id);
                cur_sdo_.reset();
            }
        }
    }
}

void JkCan::reset()
{
    deactive();
    lock_guard<mutex> l(sdo_mtx_);
    sdo_list_.clear();
    cur_sdo_.reset();
}
bool JkCan::has_same_sdo(SdoInfo& a)
{
    lock_guard<mutex> l(sdo_mtx_);
    if (sdo_list_.size() == 0)
        return false;
    for (auto sdo : sdo_list_)
    {
        if (sdo->frame_req_id != a.frame_req_id)
            continue;
        if (sdo->frame_size != a.frame_size)
            continue;
        if (memcmp(sdo->frame_data, a.frame_data, a.frame_size) != 0)
            continue;
        return true;
    }
    return false;
}
int JkCan::get_sdo(uint8_t slave_id, uint32_t obj_id, std::function<int(uint8_t*, int)> cb, int timeout, int retry)
{
    SdoInfo sdo;
    if (0 == sdo_frame_map_.count(slave_id))
        return -2;

    sdo.frame_req_id = sdo_frame_map_[slave_id].read_req_id;
    sdo.frame_resp_id = sdo_frame_map_[slave_id].resp_id;
    sdo.frame_data[0] = slave_id;
    if (2 == sdo_frame_map_[slave_id].obj_id_size)
    {
        sdo.frame_data[1] = (obj_id >> 8) & 0xff;
        sdo.frame_data[2] = (obj_id)&0xff;
        sdo.frame_size = 3;
        sdo.frame_obj_size = 2;
    }
    else if (3 == sdo_frame_map_[slave_id].obj_id_size)
    {
        // SCB 索引存在子索引，长度为3
        sdo.frame_data[1] = (obj_id >> 16) & 0xff;
        sdo.frame_data[2] = (obj_id >> 8) & 0xff;
        sdo.frame_data[3] = (obj_id)&0xff;
        sdo.frame_size = 4;
        sdo.frame_obj_size = 3;
    }
    else
    {
        return -2;
    }

    if (has_same_sdo(sdo))
        return -1;

    sdo.slave_id = slave_id;
    sdo.obj_id = obj_id;
    sdo.cb = cb;
    sdo.timeout = timeout;
    sdo.retry = retry;
    sdo.is_write = false;
    sdo.timeout_cnt = 0;
    lock_guard<mutex> l(sdo_mtx_);
    sdo_list_.push_back(make_shared<SdoInfo>(sdo));
    jkcan_info("Req read slave{} sdo:{} timeout:{} retry:{}", slave_id, obj_id, timeout, retry);
    return 0;
}
int JkCan::get_sdo_blocking(uint8_t slave_id, uint32_t objid, void* data, int data_len, int retry)
{
    const uint32_t timeout = 64;
    bool data_check = false;
    auto objlen = sdo_frame_map_[slave_id].obj_id_size;
    auto err = get_sdo(
        slave_id,
        objid,
        [data, objlen, data_len, &data_check](uint8_t* d, int size) {
            if (size != (int)(1 + objlen + data_len))
                return -1;
            uint8_t big_end[8] = {};
            for (int i = 0; i < data_len; i++) { big_end[data_len - i - 1] = d[1 + objlen + i]; }
            memcpy(data, big_end, data_len);
            data_check = true;
            return 0;
        },
        timeout,
        retry);
    if (err)
        return err;

    for (int i = 0; i < retry + 3;)
    {
        if (data_check == true)
        {
            return 0;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(timeout));
        if (retry >= 0)
        {
            i++;
        }
    }
    return -10;
}
int JkCan::set_sdo(uint8_t slave_id, uint32_t obj_id, uint8_t val[4], size_t size, std::function<int(uint8_t*, int)> cb, int timeout, int retry)
{
    if (size > 4)
        return -3;
    SdoInfo sdo;
    if (0 == sdo_frame_map_.count(slave_id))
        return -2;

    sdo.frame_req_id = sdo_frame_map_[slave_id].write_req_id;
    sdo.frame_resp_id = sdo_frame_map_[slave_id].resp_id;
    sdo.frame_data[0] = slave_id;
    if (2 == sdo_frame_map_[slave_id].obj_id_size)
    {
        sdo.frame_data[1] = (obj_id >> 8) & 0xff;
        sdo.frame_data[2] = (obj_id)&0xff;
        sdo.frame_size = 3 + size;
        sdo.frame_obj_size = 2;
        for (size_t i = 0; i < size; i++) { sdo.frame_data[3 + i] = val[size - i - 1]; }
    }
    else if (3 == sdo_frame_map_[slave_id].obj_id_size)
    {
        sdo.frame_data[1] = (obj_id >> 16) & 0xff;
        sdo.frame_data[2] = (obj_id >> 8) & 0xff;
        sdo.frame_data[3] = (obj_id)&0xff;
        sdo.frame_size = 4 + size;
        sdo.frame_obj_size = 3;
        for (size_t i = 0; i < size; i++) { sdo.frame_data[4 + i] = val[size - i - 1]; }
    }
    else
    {
        return -2;
    }

    if (has_same_sdo(sdo))
        return -1;

    sdo.slave_id = slave_id;
    sdo.obj_id = obj_id;
    sdo.cb = cb;
    sdo.timeout = timeout;
    sdo.retry = retry;
    sdo.is_write = true;
    sdo.timeout_cnt = 0;
    lock_guard<mutex> l(sdo_mtx_);
    sdo_list_.push_back(make_shared<SdoInfo>(sdo));
    jkcan_info("Req write slave{} sdo:{} timeout:{} retry:{}", slave_id, obj_id, timeout, retry);
    return 0;
}
JkCan::SdoInfo* JkCan::get_sdo_req_data()
{
    if (!cur_sdo_.get())
    {
        //从队列中获取最新的到cur_sdo_
        if (sdo_list_.size())
        {
            cur_sdo_ = sdo_list_.front();
            sdo_list_.pop_front();
        }
        else
            return nullptr;
    }
    //检查是否超时
    if (cur_sdo_->timeout_cnt == 0)  //第一次
    {
        cur_sdo_->timeout_cnt += cyclic_time_us_;
        return cur_sdo_.get();
    }
    else if (cur_sdo_->timeout_cnt <= cur_sdo_->timeout * 1000)  //正在处理中
    {
        cur_sdo_->timeout_cnt += cyclic_time_us_;
        return nullptr;
    }
    else if (cur_sdo_->timeout_cnt > cur_sdo_->timeout * 1000)  //超时了
    {
        if (cur_sdo_->retry >= 0)
        {
            if (cur_sdo_->retry == 0)  //重试次数用完
            {
                jkcan_error("Timeout: slave{} {} sdo {}", cur_sdo_->slave_id, cur_sdo_->is_write ? "write" : "read", cur_sdo_->obj_id);
                cur_sdo_->cb(nullptr, -1);

                //从队列中获取最新的到cur_sdo_
                lock_guard<mutex> l(sdo_mtx_);
                if (sdo_list_.size())
                {
                    cur_sdo_ = sdo_list_.front();
                    sdo_list_.pop_front();
                }
                else
                {
                    cur_sdo_.reset();
                }
                return nullptr;
            }
            else  //进行重试
            {
                jkcan_debug("Retry {}: req slave{} to {} sdo {}", cur_sdo_->retry, cur_sdo_->slave_id, cur_sdo_->is_write ? "write" : "read", cur_sdo_->obj_id);
                cur_sdo_->timeout_cnt = cyclic_time_us_;
                cur_sdo_->retry--;
                return cur_sdo_.get();
            }
        }
    }
    return nullptr;
}

int JkCan::register_sdo_handle(uint8_t slave_id, uint32_t write_req_frame_id, uint32_t read_req_frame_id, uint32_t resp_frame_id, size_t obj_id_size)
{
    if (sdo_frame_map_.count(slave_id))
        return -1;
    SdoFrame f;
    f.write_req_id = write_req_frame_id;
    f.read_req_id = read_req_frame_id;
    f.resp_id = resp_frame_id;
    f.obj_id_size = obj_id_size;
    f.slave_id = slave_id;
    sdo_frame_map_[slave_id] = f;
    return 0;
}